
#ifndef NAV_DWB_CRITICS__TWIRLING_HPP_
#define NAV_DWB_CRITICS__TWIRLING_HPP_

#include "nav_dwb_controller_pkg/trajectory_critic.hpp"

namespace nav_dwb_critics
{
/**
 * @class TwirlingCritic
 * @brief Penalize trajectories with rotational velocities
 *
 * This class provides a cost based on how much a robot "twirls" on its way to the goal. With
 * differential-drive robots, there isn't a choice, but with holonomic or near-holonomic robots,
 * sometimes a robot spins more than you'd like on its way to a goal. This class provides a way
 * to assign a penalty purely to rotational velocities.
 */
class TwirlingCritic : public nav_dwb_controller_pkg::TrajectoryCritic
{
public:
  void onInit() override;
  double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
};
}  // namespace nav_dwb_critics

#endif  // NAV_DWB_CRITICS__TWIRLING_HPP_
